//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2003-2007
//
//  Geometric algorithms for trimming.
//

# include "StdAfx.h"
# include "PtTrim.h"
# include "PtAlgo.h"
# include "Map.h"
# include "Wrappers.h"

class CTrimmigCtx {
  public:
	void Set (map_t * _pMap, const epoints_t & _epoints, bool _bAddExternalNodes) {
		Reset ();

		m_pMap = _pMap;
		m_bAddExternalNodes = _bAddExternalNodes;

		// Add existing RG nodes (to preserve them securely while trimming).
		const size_t cPoints = _epoints.size ();
		m_epoints.reserve (cPoints);
		for (size_t c = 0; c < cPoints; ++ c) {
			const epoint_t & epoint = _epoints [c];
			if (! epoint.extra.empty ())
				m_epoints.push_back (epoint);
		}
	}

	void Reset () {
		m_pMap = NULL;
		m_epoints.clear ();
	}

	epoint_t * FindNode (const point_t & _point, rgn_t * _pRgn) {
		if (m_pMap == NULL)
			return NULL;

		const size_t cPoints = m_epoints.size ();
		for (size_t c = 0; c < cPoints; ++ c) {
			epoint_t & epoint = m_epoints [c];
			if (
				::fabs (epoint.point.x - _point.x) < 1e-5f &&
				::fabs (epoint.point.y - _point.y) < 1e-5f
			)
				return & epoint;
		}

		// Create trim node.
		rgnode_t & nodeNew = m_pMap->CreateNode ();
		nodeNew.bIsTrimNode = true;
		if (m_bAddExternalNodes)
			nodeNew.bIsExternal = true;

		m_epoints.push_back (epoint_t ());
		epoint_t & epointNew = m_epoints.back ();
		epointNew.point = _point;
		epointNew.extra.SetRGNode (& nodeNew, true);

		if (_pRgn && (! _pRgn->pExtra || ! _pRgn->pExtra->bOneWay)) {
			// Add U-turn restriction.
			nodeNew.bHasRestrictions = true;

			rgrestriction_t & restriction1 = AddRGNodeRestriction (* m_pMap, & nodeNew);
			restriction1.pFromRoad      = _pRgn;
			restriction1.pToRoad        = _pRgn;
			restriction1.bFromDirection = false;
			restriction1.bToDirection   = true;

			rgrestriction_t & restriction2 = AddRGNodeRestriction (* m_pMap, & nodeNew);
			restriction2.pFromRoad      = _pRgn;
			restriction2.pToRoad        = _pRgn;
			restriction2.bFromDirection = true;
			restriction2.bToDirection   = false;
		}

		return & epointNew;
	}

  private:
	std::vector<epoint_t> m_epoints;
	map_t * m_pMap;
	bool m_bAddExternalNodes;
};

static CTrimmigCtx g_TrimmigCtx;

void SetTrimmingCtx (map_t * _pMap, const epoints_t & _epoints, bool _bAddExternalNodes) {
	g_TrimmigCtx.Set (_pMap, _epoints, _bAddExternalNodes);
}

void ResetTrimmingCtx () {
	g_TrimmigCtx.Reset ();
}

////////////////////////////////////////////////

	// P axis is line AB while Q one is orthogonal to P.
# define GetP(_x,_y) ( (_x - _A.x)*fCos + (_y - _A.y)*fSin)
# define GetQ(_x,_y) (-(_x - _A.x)*fSin + (_y - _A.y)*fCos)

	// Trim polyline with right semi-plan AB.
/*void TrimPolyline (const point_t & _A, const point_t & _B, const points_t & _pointsIn, polypoints_t & _pointsOut) {
	// Prepare input points.
	epoints_t epointsIn;
	Points2EPoints (_pointsIn, NULL, epointsIn);

	// Trim.
	polyepoints_t pointsOut;
	TrimPolyline (_A, _B, epointsIn, pointsOut);

	// Prepare output points.
	for (polyepoints_t::const_iterator i = pointsOut.begin (); i != pointsOut.end (); ++ i) {
		const epoints_t & epoints = * i;

		points_t points;
		point_extras_t PointExtras;
		EPoints2Points (epoints, false, points, PointExtras);

		_pointsOut.push_back (points_t ());
		_pointsOut.back ().swap (points);
	}
}*/

void TrimPolyline (const point_t & _A, const point_t & _B, const epoints_t & _epointsIn, bool _bHasRGNodes, polyepoints_t & _epointsOut, rgn_t * _pRgn) {
	const size_t cPoints = _epointsIn.size ();
	if (cPoints == 0)
		return;

	// Prepare coordinate transformation (X,Y)->(P,Q).
	const double fC = ::hypot (_B.x - _A.x, _B.y - _A.y);
	assert (fC > 0);
	if (fC == 0)
		return;
	const double fCos = (_B.x - _A.x)/fC;
	const double fSin = (_B.y - _A.y)/fC;

	// Create new polygon's part.
	if (_epointsOut.empty () || ! _epointsOut.back ().empty ())
		_epointsOut.push_back (epoints_t ());
	epoints_t * pPoints = & _epointsOut.back ();

	for (size_t cPoint = 0; cPoint < cPoints - 1; ++ cPoint) {
		const epoint_t & pt0 = _epointsIn [cPoint];
		const epoint_t & pt1 = _epointsIn [cPoint + 1];

		const double q0 = GetQ (pt0.point.x, pt0.point.y);
		const double q1 = GetQ (pt1.point.x, pt1.point.y);
		const bool bInside0 = q0 <= 0;
		const bool bInside1 = q1 <= 0;

		if (bInside0)
			// Output this point.
			pPoints->push_back (pt0);

		if (bInside0 != bInside1) {
			if (bInside1) {
				RemoveRepeatingPoints (* pPoints);

				// Start new polyline.
				if (! pPoints->empty ())
					_epointsOut.push_back (epoints_t ());
				pPoints = & _epointsOut.back ();
			}

			// Get the cross point (i.e. q = 0).
			const double p0 = GetP (pt0.point.x, pt0.point.y);
			const double p1 = GetP (pt1.point.x, pt1.point.y);

			assert (q1 != q0);
			const double p = (p0*q1 - p1*q0)/(q1 - q0);

			epoint_t ptCross;
			ptCross.point.x = _A.x + p*fCos;
			ptCross.point.y = _A.y + p*fSin;
			if (_bHasRGNodes) {
				epoint_t * const pEPoint = g_TrimmigCtx.FindNode (ptCross.point, _pRgn);
				if (pEPoint) {
					ptCross.point = pEPoint->point;
					ptCross.extra.SetRGNode (pEPoint->extra.GetRGNode (), true);
				}
			}

			pPoints->push_back (ptCross);
		}

		if (cPoint == cPoints - 2 && bInside1)
			pPoints->push_back (pt1);
	}

	if (! _epointsOut.empty () && _epointsOut.back ().empty ())
		_epointsOut.pop_back ();
}

void TrimPolylineOutsideRect (const rect_t & _rectMask, const epoints_t & _epointsIn, bool _bHasRGNodes, polyepoints_t & _epointsOut, rgn_t * _pRgn) {
	// NOTE: rectangle mask is superposition of 4 semi-plans.
	struct {
		float Ax;
		float Ay;
		float Bx;
		float By;
	} lines [] = {
		{_rectMask.x0, _rectMask.y0, _rectMask.x0, _rectMask.y1},
		{_rectMask.x0, _rectMask.y1, _rectMask.x1, _rectMask.y1},
		{_rectMask.x1, _rectMask.y1, _rectMask.x1, _rectMask.y0},
		{_rectMask.x1, _rectMask.y0, _rectMask.x0, _rectMask.y0},
	};

	polyepoints_t epointsIn;
	epointsIn.push_back (_epointsIn);

	SetTrimmingCtx (& g_map, _epointsIn, true);

	polyepoints_t epointsOut;
	for (size_t cSemiPlan = 0; cSemiPlan < 4; ++ cSemiPlan) {
		for (polyepoints_t::const_iterator iPart = epointsIn.begin (); iPart != epointsIn.end (); ++ iPart) {
			TrimPolyline (
				point_t (lines [cSemiPlan].Ax, lines [cSemiPlan].Ay), point_t (lines [cSemiPlan].Bx, lines [cSemiPlan].By),
				* iPart, _bHasRGNodes, 
				epointsOut,
				_pRgn
			);
		}
		epointsIn.swap (epointsOut);
		epointsOut.clear ();
	}

	epointsIn.swap (_epointsOut);

	ResetTrimmingCtx ();
}

////////////////////////////////////////////

struct cross_t {
	double fP;
	point_t pt;
	size_t cLegIndex;
	bool bOut;

	bool operator < (const cross_t & _other) const {return fP < _other.fP;}
};

	// Trim polygon with right semi-plan AB.
void TrimPolygon (const point_t & _A, const point_t & _B, const points_t & _pointsIn, polypoints_t & _pointsOut) {
	const size_t cPoints = _pointsIn.size ();
	if (cPoints == 0)
		return;

///////////////////////
//	assert (! PolylineIntersectsSelf (_pointsIn, true));

# if 0
//# if defined (_DEBUG)
{
static size_t s_cPass = 0;
char strFileName [MAX_PATH];
::sprintf (strFileName, "f:\\%03d.mp", s_cPass ++);

FILE * fp = fopen (strFileName, "wt");
fprintf (fp, "[IMG ID]\nTypeSet=Navitel\nLevels=2\nLevel0=26\nLevel1=20\nZoom0=0\nZoom1=1\n[END-IMG ID]\n");
fprintf (fp, "[POLYGON]\nType=0x50\nData0=");
for (size_t n = 0; n < _pointsIn.size (); ++ n)
	fprintf (fp, "(%.5f,%.5f),", _pointsIn [n].y, _pointsIn [n].x);
fprintf (fp, "\n[END-POLYGON]\n");
fclose (fp);
}
# endif // _DEBUG

	const bool bHuge = cPoints > 10000;

	// Make sure that polygon is right-oriented.
	point_t * const pPointsIn = bHuge ? new point_t [cPoints] : reinterpret_cast<point_t *> (alloca (sizeof (point_t)*cPoints));
	CArrayHolder<point_t> pPointsIn_Holder (bHuge ? pPointsIn : NULL);
	std::copy (_pointsIn.begin (), _pointsIn.end (), pPointsIn);
	if (! IsRightOriented (_pointsIn))
		std::reverse (pPointsIn, pPointsIn + cPoints);

	// Prepare coordinate transformation (X,Y)->(P,Q).
	const double fC = ::hypot (_B.x - _A.x, _B.y - _A.y);
	assert (fC > 0);
	if (fC == 0)
		return;
	const double fCos = (_B.x - _A.x)/fC;
	const double fSin = (_B.y - _A.y)/fC;

	//
	// The first pass: make set of cross points sorted by P.
	//
	cross_t * const pCrosses = bHuge ? new cross_t [cPoints] : reinterpret_cast<cross_t *> (alloca (sizeof (cross_t)*cPoints));
	CArrayHolder<cross_t> pCrosses_Holder (bHuge ? pCrosses : NULL);
	size_t cCrosses = 0;

	bool bAtLeastOneInside = false;

	size_t cPoint;
	for (cPoint = 0; cPoint < cPoints; ++ cPoint) {
		const point_t & pt0 = pPointsIn [cPoint];
		const point_t & pt1 = pPointsIn [cPoint + 1 < cPoints ? cPoint + 1 : 0];

		const double q0 = GetQ (pt0.x, pt0.y);
		const double q1 = GetQ (pt1.x, pt1.y);

		if (q0 <= 0)
			bAtLeastOneInside = true;

		if ((q0 <= 0) != (q1 <= 0)) {
			// Get the cross point (i.e. q = 0).
			const double p0 = GetP (pt0.x, pt0.y);
			const double p1 = GetP (pt1.x, pt1.y);

			assert (q1 != q0);
			const double p = (p0*q1 - p1*q0)/(q1 - q0);
			const point_t ptCross (_A.x + p*fCos, _A.y + p*fSin);

			// Save the cross data.
			assert (cCrosses < cPoints);
			cross_t & cross = pCrosses [cCrosses ++];
			cross.fP        = p;
			cross.pt        = ptCross;
			cross.cLegIndex = cPoint;
			cross.bOut = q0 <= 0;
		}
	}

	// Optimization for simple case of non-intersection.
	if (cCrosses == 0) {
		if (bAtLeastOneInside) {
			_pointsOut.push_back (points_t ());
			_pointsOut.back ().assign (pPointsIn, pPointsIn + cPoints);
		}
		return;
	}

	// NOTE: number of crosses is even: each 'in' matches 'out'.
	assert ((cCrosses & 0x1) == 0);

	// Complete crosses preparing: sort them by P value.
	std::sort (pCrosses, pCrosses + cCrosses);

	// Fix ins & out order if P is equal.
	// NOTE: it is supposed that matched 'in's and 'out's are paired in pCrosses:
	// 'outs's are even and 'in's are odd.
	// TODO: handle more complex case with > 2 cross points with equal P.
	size_t cCross = 0;
	for (cCross = 0; cCross < cCrosses - 1; ++ cCross) {
		assert (pCrosses [cCross].fP <= pCrosses [cCross + 1].fP);

		if (pCrosses [cCross].fP == pCrosses [cCross + 1].fP) {
			const bool bEven = (cCross & 0x1) == 0;
			if (pCrosses [cCross].bOut != bEven) {
				std::swap (pCrosses [cCross], pCrosses [cCross + 1]);
				++ cCross;
			}
		}
	}

	size_t * const pLeg2Cross = bHuge ? new size_t [cPoints] : reinterpret_cast<size_t *> (alloca (sizeof (size_t)*cPoints));
	CArrayHolder<size_t> pLeg2Cross_Holder (bHuge ? pLeg2Cross : NULL);
# ifdef _DEBUG
	std::fill (pLeg2Cross, pLeg2Cross + cPoints, -1);
# endif // _DEBUG
	for (cCross = 0; cCross < cCrosses; ++ cCross) {
		const size_t cPoint = pCrosses [cCross].cLegIndex;
		assert (cPoint < cPoints);
		pLeg2Cross [cPoint] = cCross;
	}

	//
	// The second pass: cut all outside.
	//

	bool * const pVisited = bHuge ? new bool [cPoints] : reinterpret_cast<bool *> (alloca (sizeof (bool)*cPoints));
	CArrayHolder<bool> pVisited_Holder (bHuge ? pVisited : NULL);
	std::fill (pVisited, pVisited + cPoints, false);

	size_t cVisitedPoints = 0;
	size_t cFirstPointToVisit = 0;
	do {
		// Omit already visited.
		for (; cFirstPointToVisit < cPoints; ++ cFirstPointToVisit)
			if (! pVisited [cFirstPointToVisit])
				break;

		// Create new polygon's part.
		if (_pointsOut.empty () || ! _pointsOut.back ().empty ())
			_pointsOut.push_back (points_t ());
		points_t & points = _pointsOut.back ();

		// Visit rest of points.
		for (cPoint = cFirstPointToVisit; cPoint < cPoints; ++ cPoint) {
			if (pVisited [cPoint])
				break;
			pVisited [cPoint] = true;
			++ cVisitedPoints;

			const point_t & pt0 = pPointsIn [cPoint];
			const point_t & pt1 = pPointsIn [cPoint + 1 < cPoints ? cPoint + 1 : 0];

			const bool bInside0 = GetQ (pt0.x, pt0.y) <= 0;
			const bool bInside1 = GetQ (pt1.x, pt1.y) <= 0;

			if (bInside0) {
				// Output this point.
				points.push_back (pt0);

				if (! bInside1) {
					// Output the 'out' and following 'in' cross points.
					size_t cCrossOut = pLeg2Cross [cPoint];
					assert (cCrossOut != -1);
					assert (pCrosses [cCrossOut].bOut);
# ifdef _DEBUG
					pLeg2Cross [cPoint] = -1;
# endif // _DEBUG

					// NOTE: it is assumed that 'outs's are even.
					const bool bOutIsEven = (cCrossOut & 0x1) == 0;
# ifdef _DEBUG
					if (! bOutIsEven) {
# if 0
//# if 1
static size_t s_cPass = 0;
char strFileName [MAX_PATH];
::sprintf (strFileName, "f:\\a%03d.mp", s_cPass ++);

FILE * fp = fopen (strFileName, "wt");
fprintf (fp, "[IMG ID]\nTypeSet=Navitel\nLevels=2\nLevel0=26\nLevel1=20\nZoom0=0\nZoom1=1\n[END-IMG ID]\n");
fprintf (fp, "[POLYGON]\nType=0x50\nData0=");
for (size_t n = 0; n < _pointsIn.size (); ++ n)
	fprintf (fp, "(%.5f,%.5f),", _pointsIn [n].y, _pointsIn [n].x);
fprintf (fp, "\n[END-POLYGON]\n");
fclose (fp);
# endif
						assert (bOutIsEven);
						// Odd 'out' may be due to self-intersection.
						assert (PolylineIntersectsSelf (_pointsIn, true));
					}
# endif // _DEBUG
					const cross_t & crossOut = pCrosses [cCrossOut];
					points.push_back (crossOut.pt);

					size_t cCrossIn = cCrossOut + 1;
					if (! bOutIsEven && cCrosses == 2) {
						// Ad hoc fix for most typical and simple case.
						assert (cCrossIn == 2);
						cCrossIn = 0;
					}
					assert (cCrossIn < cCrosses);
					if (cCrossIn >= cCrosses)
						break;
					const cross_t & crossIn  = pCrosses [cCrossIn];
					points.push_back (crossIn.pt);

					// Omit all outside points till 'in' cross one.
					cPoint = crossIn.cLegIndex;
				}
			}
		}

		// Remove repeating points.
		RemoveRepeatingPoints (points);
	} while (cVisitedPoints < cPoints);

	if (! _pointsOut.empty () && _pointsOut.back ().empty ())
		_pointsOut.pop_back ();
}

void TrimPolygon (const point_t & _A, const point_t & _B, const epoints_t & _epointsIn, polyepoints_t & _epointsOut) {
	// Prepare input points.
	points_t pointsIn;
	point_extras_t PointExtras;
	EPoints2Points (_epointsIn, false, pointsIn, PointExtras);

	// Trim.
	polypoints_t pointsOut;
	TrimPolygon (_A, _B, pointsIn, pointsOut);

	// Prepare output points.
	for (polypoints_t::const_iterator i = pointsOut.begin (); i != pointsOut.end (); ++ i) {
		const points_t & points = * i;

		epoints_t epoints;
		Points2EPoints (points, NULL, epoints);

		_epointsOut.push_back (epoints_t ());
		_epointsOut.back ().swap (epoints);
	}
}

void TrimPolygonOutsideRect (const rect_t & _rectMask, const epoints_t & _epointsIn, polyepoints_t & _epointsOut) {
	// NOTE: rectangle mask is superposition of 4 semi-planes.
	struct {
		float Ax;
		float Ay;
		float Bx;
		float By;
	} lines [] = {
		{_rectMask.x0, _rectMask.y0, _rectMask.x0, _rectMask.y1},
		{_rectMask.x0, _rectMask.y1, _rectMask.x1, _rectMask.y1},
		{_rectMask.x1, _rectMask.y1, _rectMask.x1, _rectMask.y0},
		{_rectMask.x1, _rectMask.y0, _rectMask.x0, _rectMask.y0},
	};

	polyepoints_t epointsIn;
	epointsIn.push_back (_epointsIn);

	polyepoints_t epointsOut;
	for (size_t cSemiPlan = 0; cSemiPlan < 4; ++ cSemiPlan) {
		for (polyepoints_t::const_iterator iPart = epointsIn.begin (); iPart != epointsIn.end (); ++ iPart) {
			TrimPolygon (
				point_t (lines [cSemiPlan].Ax, lines [cSemiPlan].Ay), point_t (lines [cSemiPlan].Bx, lines [cSemiPlan].By),
				* iPart, epointsOut
			);
		}
		epointsIn.swap (epointsOut);
		epointsOut.clear ();
	}

	epointsIn.swap (_epointsOut);
}
